COMMENT ⊗   VALID 00009 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	.TITLE POINTY
C00004 00003	ROUTINE TO EXECUTE A MOVE WITH NO 'AL ROUTINES
C00008 00004	 LOOK IF ANYONE HIT KEYBOARD, DO A WHERE, AND SEE IF MOTION WANTED
C00011 00005	ROUTINE TO DISPLAY CURRENT JOINT ANGLES
C00015 00006	ROUTINE TO DISPLAY CURRENT REFERENCE POWER SUPPLY READINGS
C00017 00007	FORCE SENSING/COMPLIANCE PATCH AREA
C00019 00008	SIMULTANEOUS HAND OPERATION
C00021 00009	LOCAL STORAGE AREA
C00027 ENDMK
C⊗;
.TITLE POINTY

TRANS ==1	;SET TO 1 TO PRINT OUT EULER AS WELL AS JOINT ANGLES
HANDOP==0	;=1 FOR SIMULTANEOUS HAND OPERATION

SNGSTP==0	;CONDITIONAL ASSEMBLY FLAGS FOR ARM.PAL
DIAGY ==0
STDALN==1
TACCAL==0
NOYELW==1
TIMER ==0
ISLIN ==1
FRCDAT==0

.INSRT K1DEF.PAL[11,SYS]
.INSRT ALHEAD.PAL[AL,HE]
.INSRT ARM.PAL[AL,HE]
.INSRT BEJCZY.PAL[AL,HE]
.INSRT ARITH.PAL[AL,HE]
.INSRT IO.PAL[3,BES]
.IFNZ TRANS
   .INSRT ARMSOL.PAL[al,he]
   .INSRT EULER.PAL[AL,HE]
.IFF
   UPDATE:
   SOLVE:	0
.ENDC


;COMMUNICATION LINK FOR "TLKEF5", "WHERE" ROUTINE FOR THE PDP10

MASLC5==110100	;ADDRESS OF MASTER NUMBER TO SIGNAL PDP10 WE ARE ALIVE
TRNADR==110102	;SAVE TRANSFORM DATA IN HERE
JTSADR==110162	;SAVE JOINT ANGLES IN HERE

MASTR5==12121	;CHECK NUMBER TO PDP10

;COMMUNICATION LOCATION BETWEEN MOVE.SAI AND MOVE.PAL

MCLOC==	100000

;DEVICE MECHANISM BITS

YELARM== 1	;MECHANISM BITS: YELLOW ARM, NOT INCLUDING HAND
YELHND== 2 	;YELLOW HAND
BLUARM== 4	;BLUE ARM, NOT INCLUDING HAND
BLUHND== 10	;BLUE HAND
;ROUTINE TO EXECUTE A MOVE WITH NO 'AL ROUTINES

POINTY:	CLR	JMPFRC		;NO FORCE COMPLIANCE
	MOV	#START,JOBSA
	JMP	1000

START:	MOV	#DEVICE,R1	;INITIALIZE BLUE ARM TO RUN
	JSR	PC,INTARM
	TST	R0		;CHECK FOR ERRORS
	BEQ	PHEADR		;BRANCH IF NO INITIALIZATION ERROR

BADINT:	MOV	#BADSTR,SG	;ELSE TYPE ERROR MESSAGE
	JSR	PC,TYPERR
	DISMIS

PHEADR:	JSR	PC,PRTHDR	;PRINT HEADER INFORMATION
	CLR	VT05		; RESET VT05 FLAG
	FORK	#LOOK,#MLOOP,#USRDM

PRTW:	MOV	#COMWH,SG	;JUMP TOP LINE
	JSR	PC,TYPSTR
      	MOV	#IOBUF,SG	;START BUILDING OUTPUT STRING IN HERE
	MOV	#BTH,R2		;GET POINTERS JOINT READINGS
	MOV	#7,R3		;7 JOINTS IN ALL
JTCLP:	LDF 	@(R2)+,AC0	;GET JOINT ANGLE
	JSR	PC,CVF		;CONVERT JOINT ANGLE TO STRING 
	SOB	R3,JTCLP
	MOVB	#15,(SG)+	;PUT IN A CR LF
	MOVB	#12,(SG)+
	MOV	#9.,R0		;CHANGE STRING FORMAT FOR INTEGER NUMBER
	CLR	R1
	JSR	PC,FORMAT
	MOV	#BSRVOS,R1	;GET POINTERS TO SERVOS
	MOV	#7,R3		;CONVERT THE ACTUAL POT READINGS
NEXTP:	MOV	(R1)+,R0
	LDCIF	POT(R0),AC0	;GET A/D READING
	TST	POT(R0)		;CHECK IF ROUND UP OR DOWN
	BLT	RNDDN
	ADDF	#40000,AC0	;ROUND OFF READING
	BR	.+6
RNDDN:	SUBF	#40000,AC0
	JSR	PC,CVF		;CONVERT TO ASC
	SOB	R3,NEXTP
	JSR	PC,RSTFOR
	CLRB	(SG)
	MOV	#IOBUF,SG	;TYPE OUT WHOLE STRING
	JSR	PC,TYPSTR
   .IFNZ TRANS
	MOV	#TRNADR,R0	;GET CURRENT TRANSFORM IN HERE
	MOV	#THPTR,R1	;POINT TO THE CURRENT JOINT ANGLES
	MOV	#BLUARM,R2	;USE SOLUTION FOR BLUE ARM
	JSR	PC,UPDATE	;COMPUTE THE CURRENT TRANSFORM
	MOV	#TRNADR,R0	;COMPUTE THE EULER ANGLES
	MOV	#EANGLE,R1	;PUT THEM IN HERE
	JSR	PC,EULER
	MOV	#IOBUF,SG	;CONVERT TO STRING FOR OUTPUT
	MOVB	#15,(SG)+	;PUT IN A CR LF LF
	MOVB	#12,(SG)+
	MOVB	#12,(SG)+
	MOV	#6,R0
	MOV	#EANGLE,R1
EANGC:	LDF	(R1)+,AC0
	JSR	PC,CVF
	SOB	R0,EANGC	;REPEAT TILL DONE
	CLRB	(SG)
	MOV	#IOBUF,SG	;TYPE OUT THE EULER ANGLES
	JSR	PC,TYPSTR
	MOV	#BTH,R0		;TRANSFER THE JOINT ANGLES TO PDP10 
	MOV	#JTSADR,R1	;   COMMUNICATIONS TABLE
	MOV	#7,R2		;6 JOINTS + HAND
RHTLP:	LDF	@(R0)+,AC0	
	STF	AC0,(R1)+
	SOB	R2,RHTLP
	MOV	#MASTR5,MASLC5	;INDICATE TO PDP10 THAT "WHERE" IS WORKING
   .ENDC
	TST	PRTDVB		;PRINT OUT DEVICE BLOCK?
	BEQ	$2
	JSR	PC,PRTDB	;YES, JUMP TO RELEVANT ROUTINE
$2:	TST	VT05
	BEQ	$3
	DISMIS
$3:	JMP	PRTW
; LOOK IF ANYONE HIT KEYBOARD, DO A WHERE, AND SEE IF MOTION WANTED

LOOK:	PDBLK	2,100,FP	; SET UP PBD FOR PROCESS THAT LOOKS AT MASTER NO
				; READS ARM POSITIONS AND CHECKS IF ANYTHING FROM 10
MLOOP:	TST	KBIS		;CHECK IF ANYONE HIT VT05 KEYBOARD
	BEQ	ELFRDY
	CLRB	KBIR
	CLR	MASLC5		;INDICATE "WHERE" NOT ACTIVE
	MOV	#1,VT05		;INDICATE INTERRUPT FROM VT05
	DISMIS

ELFRDY:	CMP	#54321,MCLOC	;CHECK IF DATA BLOCK ARRIVED FROM 10
	BEQ	MOTION
	MOV	#12345,MCLOC    ;INDICATE ELF READY FOR DATA BLOCK
	MOV	#BARM+BHAND,WLST	;DO A WHERE FOR THE BLUE ARM
       	MOV	#WLST,R0
	MOV	#DEVICE,R1
	JSR	PC,WHERE	;READ THE JOINT POSITIONS
	TST	R0		;CHECK IF ANY ERRORS
	BEQ	$1		;BRANCH IF OK
	MOV	#BADW,SG	;ELSE TYPE ERROR MESSAGE
	JSR	PC,TYPERR
$1:	SLEEP	#20		; GO TO SLEEP FOR 20 MSECS
	BR	MLOOP

;START OF MOVE CODE

MOTION:	MOV	#12345,MCLOC    ;INDICATE ELF ALIVE FOR DATA BLOCK
	MOV	#MCLOC+4,R0	;POINT TO COEFFICIENT LIST
	MOV	#DEVICE,R1	;POINT TO DEVICE LIST
	CMP	#76,MCLOC+2	;MOVE OR CENTER?
	BNE	CENCOD
	JSR	PC,MOVE
	JMP	CENCD2
CENCOD:	JSR	PC,CENTER
CENCD2:	MOV	#1,PRTDVB	;PRINT OUT NEW DEVICE BLOCK
	MOV	R0,STATSV	;SAVE STATUS WORD
	JMP	MLOOP

;ROUTINE TO DISPLAY CURRENT JOINT ANGLES
;START OF ROUTINE TO PRINT WHERE LINES

TYPERR:	JSR	PC,TYPSTR
	MOV	#IOBUF,SG	;TYPE ERROR CODE
	JSR	PC,CVO
	MOV	#IOBUF,SG
	JSR	PC,TYPSTR
	JSR	PC,CRLF
	CLR	MASLC5		;INDICATE W NOT WORKING
	RTS	PC

PRTHDR: MOV	#COMHD,SG	;GO TO TOP OF PAGE
	JSR	PC,TYPSTR
	MOV	#HEADER,SG	;TYPE OUT COLUMN HEADER
	JSR	PC,TYPSTR
	MOV	#8.,R0
	MOV	#2,R1
	JSR	PC,FORMAT
	RTS	PC

PRTDB:	CLR	PRTDVB
	CMP	#0,STATSV
	BNE	$4
	MOV	#CLRSCR,SG
	JSR	PC,TYPSTR
	MOV	#SUCCES,SG	;SUCCESSFUL MOVE
	JSR	PC,TYPSTR	;PRINT RELEVANT MESSAGE
	RTS	PC

$4:	MOV	#NOSCCS,SG	;TOO BAD UNSUCCESSFUL MOVE
	JSR	PC,TYPSTR
	MOV	#CLRSCR,SG
	JSR	PC,TYPSTR
	MOV	STATSV,R0
	CMP	#15,R0		;LOOK AT LOW BYTE ERRORS
	BLT	$5

	ASL	R0		;THIS IS LOW BYTE ERROR
	MOV	ERRMES(R0),SG
	JSR	PC,TYPSTR	;PRINT OUT RELEVANT MESSAGE
	JSR	PC,CRLF
	JMP	$8

$5:	MOV	#7,R1		;THIS IS HIGH BYTE ERROR
	MOV	#16,R0
$7:	ASRB	STATSV+1
	BCC	$6
	ASL	R0
	MOV	ERRMES(R0),SG
	JSR	PC,TYPSTR
	JSR	PC,CRLF
$6:	INC	R0
	SOB	R1,$7

$8:	MOV	DEVICE,R2	;GET THE NUMBER OF  SERVOS ATTACHED
	BEQ	ENDS4		;BRANCH IF NONE ATTACHED

	MOV	#COM7,SG	;ELSE TYPE OUT THE DEVICE BLOCK CONTENTS
	JSR	PC,TYPSTR
	MOV	#DEVICE+4,R1	;START WITH THIS WORD
DEVLP:	MOV	(R1)+,R0
	MOV	#IOBUF,SG
	JSR	PC,CVO
	MOV	#IOBUF,SG
	JSR	PC,TYPSTR
	MOV	#SPACE3,SG
	JSR	PC,TYPSTR
	SOB	R2,DEVLP
ENDS4:	MOV	#CLRSCR,SG
	JSR	PC,TYPSTR
	RTS	PC
;ROUTINE TO DISPLAY CURRENT REFERENCE POWER SUPPLY READINGS

REF:	MOV	#START3,JOBSA
	JMP	1000

START3:	MOV	#COM5,SG	;TYPE OUT THE CALIB. REFERENCE READING
	JSR	PC,TYPSTR
	MOV	REFER1,R0	;GET REF. READING
	MOV	#IOBUF,SG
	JSR	PC,CVI
	MOVB	#54,(SG)+	;PUT IN A COMMA AND SPACE CHARACTER
	MOVB	#40,(SG)+
	MOV	REFER2,R0	;GET SECOND REF. READING
	JSR	PC,CVI
	MOV	#IOBUF,SG	;TYPE THEM OUT
	JSR	PC,TYPSTR
	JSR	PC,CRLF
	CLR	DEVICE		;DON'T READ ANY TACHS
REFLP:	MOV	#DEVICE,R1	;READ THE REF. POWER SUPPLY VOLTAGE
	JSR	PC,SETREF
      	MOV	#REFSG2,SG	;START BUILDING OUTPUT STRING IN HERE
	MOV	REFVT1,R0	;GET REFERENCE READINGS AND CONVERT TO ASC
	JSR	PC,CVI
	MOVB	#54,(SG)+	;PUT IN A COMMA AND SPACE CHARACTER
	MOVB	#40,(SG)+
	MOV	REFVT2,R0	;GET THE SECOND READING
	JSR	PC,CVI
	MOVB	#40,(SG)+	;PUT IN A FEW SPACE CHARACTERS
	MOVB	#40,(SG)+
	MOVB	#40,(SG)+
	MOV	#REFSTG,SG	;TYPE OUT WHOLE STRING
	JSR	PC,TYPSTR
        TST	KBIS		;CHECK IF ANYONE HIT VT05 KEYBOARD
	BEQ	REFLP
	CLRB	KBIR
	JSR	PC,CRLF
	DISMIS
;FORCE SENSING/COMPLIANCE PATCH AREA

FPAT1:	MOV	#FTABLE+BLUARM,R0;INIT FORCE SENSING SYSTEM
	MOV	#C,R1		;RELATIVE FORCE TRANS
	JSR	PC,SETC
	TST	R0		;ERROR?
	BEQ	1$
	BPT

.IFNZ 0
1$:	MOV	#BLUARM+XFORCE,R0
	LDF	FFORCE,AC0 
	JSR	PC,COMPLY
	TST	R0		;ERROR?
	BEQ	2$
	BPT
2$:	RTS	PC
	.BLKW	100

FPAT2:	RTS	PC
	.BLKW	100
.IFF
1$:	MOV	FBITS,R0
	MOV	#FPDB,R1
	MOV	#FSSUB,R2
	LDF	FFORCE,AC0
	JSR	PC,FRCSIG
	RTS	PC

	.BLKW	100

FPAT2:	MOV	FBITS,R0
	MOV	#FPDB,R1	
	JSR	PC,FRCOFF
	RTS	PC

	.BLKW	100

FSSUB:	MOV	#FMES,SG
	JSR	PC,TYPSTR
	DISMIS

FBITS:	BLUARM+XFORCE+SIGGE
FPDB:	PDBLK	3,30,FP
FMES:	.ASCIZ	/
πFORCE SENSING TRIGGERED
/
	.EVEN
.ENDC


FFORCE:	.FLT2	0.0

C:	.FLT2	1.0,0.0,0.0
	.FLT2	0.0,1.0,0.0
	.FLT2	0.0,0.0,1.0
	.FLT2	0.0,0.0,0.0
;SIMULTANEOUS HAND OPERATION

.IFNZ 	HANDOP

SHAND:	INC	HANDGO
	LDF	CI+SRV14,AC0	;PUT DYNAMIC COEF. IN DATA LIST
	LDF	CII+SRV14,AC1
	STF	AC0,HCI
	STF	AC1,HCII
	MOV	#HNDLST,R0	;HAND COEFFICIENT LIST
	MOV	#HDEVIC,R1
	JSR	PC,MOVE
	MOV	#HMES,SG	;TYPE STATUS CODE
	JSR	PC,TYPSTR
	MOV	#HIOBUF,SG
	JSR	PC,CVO
	MOV	#HIOBUF,SG
	JSR	PC,TYPSTR
	JSR	PC,CRLF
	CLR	HANDGO
	DISMISS


;LOCAL STORAGE AREA

HNDPDB:	PDBLK	3,20,FP
HIOBUF:	.BLKW	100
HDEVIC:	.BLKW	10
HANDGO:	0
HNDLST:	BHAND
	0
	0		;NO SPECIAL MODES
	0		;NO WOBBLE
	40.		;RELATIVE SEGMENT POINTER
	5000.		;MOTION TIME
	0		;NO ASSOCIATED TRANS
	0		;NO RUN CODE
	.FLT2	0.0
	.FLT2	0.0
	.FLT2	0.0
	.FLT2	20.0
	.FLT2	-30.0
	.FLT2	12.0
HCI:	.FLT2	0.0	;CI
HCII:	.FLT2	0.0	;CII
	0		;NO OTHER SEGMENTS

HMES:	.ASCIZ	/
HAND FINISHED, STATUS = /
	.EVEN

.ENDC
;LOCAL STORAGE AREA

DEVICE:	.BLKW	33	;DEVICE BLOCK
IOBUF:	.BLKW	400.
EANGLE:	.BLKW	12.
PRTMES:	0
JMPFRC:	0
PRTDVB: 0
VT05:	0
STATSV:	0
;WHERE COEFFICIENT DATA LIST

WLST:	374     ;JOINT SERVO BIT
	0

;OUTPUT STRINGS
		; TOP OF VT05 SCREEN
COMHD:	.BYTE	35,10,10,10,37,10,10,10
   .IFNZ TRANS
	.BYTE	32,10,10,10,32,10,10,10
   .ENDC
	.BYTE	0
HEADER:	.ASCII	/                         P O I N T Y
   JT 1    JT 2    JT 3    JT 4    JT 5    JT 6     HAND
  -45.0  -165.0     6.75 -395.0   -95.0  -110.0    -0.20 LOW STOPS
  190.0   -50.0    33.00  205.0    95.0   200.0     3.80 HIGH STOPS
  180.0   -90.0    14.00  -90.0    90.0     0.0		 PARK POSITION

							 CURRENT POS
							 A-D READINGS
/
   .IFNZ TRANS
        .ASCII  /     X       Y       Z       O       A       T/
   .ENDC
	.BYTE	36
	.BYTE	0
COMWH:	.BYTE	35,10,10,10,12,10,10,10,12,10,10,10,12,10,10,10,12,10,10,10,12,10,10,10,12,10,10,10
	.BYTE	0
BADW:	.ASCIZ	/
SERVO ERROR WHILE ATTEMPTING TO DO A "WHERE", ERROR CODE = /
BADSTR:	.ASCIZ	/
ARM INITIALIZATION FAILED, ERROR CODE = /
COM5:	.ASCIZ	/
REF. POWER SUPPLY READINGS DURING CALIBRATION = /
REFSTG:	.ASCII	/CURRENT REFERENCE READINGS = /
REFSG2:	.ASCII	/                /
	.BYTE	15,10,10,10,0
SUCCES:	.ASCIZ	/

LAST MOTION COMMAND WAS SUCCESSFUL
/
CLRSCR:	.BYTE	37,10,10,10
	.BYTE	0
NOSCCS:	.ASCIZ	/

LAST MOTION COMMAND WAS UNSUCCESSFUL
/
SPACE3:	.ASCIZ /   /
	.EVEN
ERRMES:	0
	ERR1
	ERR2
	ERR3
	ERR4
	ERR5
	ERR6
	ERR7
	ERR10
	ERR11
	ERR12
	ERR13
	ERR14
	ERR15
	ERR4H
	ERR1K
	ERR2K
	ERR4K
	ERR10K
	ERR20K
	ERR40K
ERR1:	.ASCIZ	/
COULD NOT ATTACH TO REQUESTED JOINT(S)/
ERR2:	.ASCIZ	/
INCORRECT NUMBER OF JOINTS REQUESTED TO BE DRIVEN/
ERR3:	.ASCIZ	/
WIPERS COULD NOT BE READ WITHIN THEIR OPERATING RANGE/
ERR4:	.ASCIZ	/
ARM SOLUTION DOES NOT EXIST/
ERR5:	.ASCIZ	/
UNKNOWN TOUCH SENSOR REQUESTED/
ERR6:	.ASCIZ	/
NO MORE FREE SLOTS IN TOUCH SENSOR EVENT LIST/
ERR7:	.ASCIZ	/
EITHER ARM POWER SUPPLY IS OFF OR ONE OF BRAKES IS NOT SET/
ERR10:	.ASCIZ	/
REFERENCE POWER SUPPLY OUT OF RANGE/
ERR11:	.ASCIZ	/
ZERO VELOCITY TACHOMETER READING OUT OF RANGE/
ERR12:	.ASCIZ	/
ATTEMPTED TO SWITCH ARMS WHILE FORCE SERVOING/
ERR13:	.ASCIZ	/
NO MORE FREE SLOTS IN FORCE SENSOR EVENT LIST/
ERR14:	.ASCIZ	/
NEED ALL 6 ARM JOINTS IN ORDER TO DO FORCE SENSING OR COMPLIANCE/
ERR15:	.ASCIZ	/
CAN'T FORCE SERVO MOTION WITHOUT POLYNOMIAL/
ERR4H:	.ASCIZ	/
JOINT IS DOWN, INOPERABLE/
ERR1K:	.ASCIZ	/
CATASTROPHIC A-D ERROR HAS OCCURRED/
ERR2K:	.ASCIZ	/
PANIC BUTTON WAS PUSHED/
ERR4K:	.ASCIZ	/
EXCESSIVE JOINT FORCE ERROR/
ERR10K:	.ASCIZ	/
FUNCTION TOOK TOO LONG TO EXECUTE/
ERR20K: .ASCIZ	/
JOINT STOPPED BEYOND STOP LIMIT/
ERR40K: .ASCIZ	/
NO ARM SOLUTION WHILE DOING FORCE COMPLIANCE/


COM7:	.ASCIZ	/
THE DEVICE BLOCK SERVO WORDS ARE AS FOLLOWS:
/
EVMES:	.ASCII	/TOUCH SENSOR EVENT TRIGGERED
/
	.BYTE	7,0
ASKFRC:	.ASCIZ	/
DO FORCE COMPLIANCE (0,1) = /
.EVEN

;MAIN PROGRAM PROCESSOR DESCRIPTOR BLOCK

MAINL:	PDBLK	1,100,FP
	
TEMP == .		
. = JOBDAT		;MAIN PROGRAM DESCRIPTOR BLOCK
	MAINL        
. = JOBSA		;START OF MAIN PROGRAM
	START
. = JOBDM		;JOB DATA AREA MEMORY MAP
	USRDM
. = TEMP		;RESTORE LOCATION COUNTER

.END POINTY